/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package javatest1;
import javax.swing.*;
/**
 *
 * @author paul
 */
public class Main {

    /**
     * @param args the command line arguments
     */
    public static void main(String[] args) {


        RoboRobot robot = new RoboRobot();
        RoboTask.robot = robot; // reference to THE robot used by all tasks
        double startTime = System.currentTimeMillis() / 1000.0;


        RoboWindow canvas = new RoboWindow();
        canvas.robot = robot;
        JFrame frame = new JFrame();
        frame.setSize(1000, 800);
        frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
        frame.getContentPane().add(canvas);                    // Here we add it to the frame
        frame.setVisible(true);

        canvas.init();  // initialize double buffering
        robot.start();
        robot.drive.x = 0;
        robot.drive.y = -24;    // behind tracking line
        robot.drive.heading = 1;   // slightly off
        robot.lineTracker.straightLine = false;
        robot.lineTracker.splitLine = true;

        boolean arm = false;

        double lastNow = 0;
        double paintNow = 0;
        while(true){
            double now = System.currentTimeMillis() / 1000.0 - startTime;
            // simulate with a fixed time slice
            if((now - lastNow) >= robot.simDt){
                lastNow += robot.simDt;
                robot.simulate();
            }
            if((now - paintNow) >= 0.10){
                paintNow = now;
                canvas.repaint();
            }

            // now regular handling (as fast as possible
            robot.handle();
            if(false){
                if(now > 2 && !arm){
                    robot.manipulator.goToPeg(6);
                    arm = true;
                }
                if(now > 5){
                    robot.manipulator.moveArmTo(90);
                }
            }
        }
        // System.out.printf("Bye!\n");
    }
}
